/*
 *  Analog Sensor Test (Sharp GP2D12)
 *  with button for logging enable.
 *
 */


// This identifies the interrupt number for our chipset:
//   0 = digial pin 2
//   1 = digital pin 3
#define INTERRUPT_NUM 1 

// set up the button pin to match the interrupt number.
#define BUTTON_PIN 3 
// set up the a pin drive the button
#define DRIVER_PIN 4 

// identify the current LED state this is volatile because 
// the toggleLed routine which changes the value will be
// called from an interrupt. This is important because 
// we'll be using this variable in our loop routine.
volatile int button_state = HIGH; 

// our sensor has a maximum time between the measurement and
// the data being ready to read.  This is just an initial 
// delay that is only added on the start of the program.
#define SENSOR_INIT_DELAY 5

// our sensor has a delay between read-times of 48ms, meaning
// new data is only available every 48 ms.
#define SENSOR_DELAY 48

// select the analog pin we're going to use
#define SENSOR_PIN A0

struct ValueToDist {
  int value;
  int dist;
};

// We need to identify values to feed to our algorithm
// and since we know that the sensor is only ranged from
// 0 - 2.5V, we can ignore half the data points, additionally
// we're going to give a relatively small sample set (compared
// to the 1024 possible values)
ValueToDist valueDistTable[] =
 { {490, 10},
   {408, 13},
   {327, 17},
   {286, 20},
   {204, 30},
   {163, 38},
   {143, 40},
   {133, 50},
   {102, 60},
   {92,	 70},
   {82,	 80} };

int getDistance( int value )
{
  ValueToDist left;
  ValueToDist right;
  float slope;
  float c;
  
  int result = -1;

  // Test for the value being out of bounds
  if (value > valueDistTable[0].value || 
      value < valueDistTable[11].value )
  {
    Serial.println("value out of bounds");
    result = -1;
  }
  // test for the value at the closer edge of valid values
  else if ( value == valueDistTable[0].value )
  {
    result = valueDistTable[0].dist;
  }
  // test for the value at the further edge of valid values
  else if ( value == valueDistTable[11].value)
  {
    result = valueDistTable[11].dist;
  }
  // handle all other points in between
  else
  {
    // loop through the table, except the last one
    for (int i = 0; i < 11; i++ )
    {
      // if the value is between the current and next point
      if ( value <= valueDistTable[i].value && 
           value > valueDistTable[i+1].value )
      {

         Serial.print("found value range, upper =");
         Serial.print(valueDistTable[i].value);
         Serial.print(" lower =");
         Serial.println(valueDistTable[i+1].value );

         // using a simple slope algorithm to find
         // the value on the line between two known
         // points
         //   value=slope * dist + c
         left=valueDistTable[i];
         right=valueDistTable[i+1];

         // calculate the slope from the known points
         slope = ( (left.value - right.value) / (left.dist - right.dist));
         // get the slope constant using one of the known points
         c = left.value-slope*left.dist;

         // get the results from the algorithm
         result=int((value - c) / slope);

         // break out of the loop, because we've found what we're looking for
         break;
      }
    }
  }  
  
  return result;
}

void toggleLed()
{
  // read in the current state of the button
  button_state=digitalRead(BUTTON_PIN);
}

void setup()
{
  // add the driver pin
  pinMode(DRIVER_PIN, OUTPUT);
  // set the driver pin to LOW
  digitalWrite(DRIVER_PIN, LOW);
  
  // add the driver pin
  pinMode(BUTTON_PIN, INPUT);
  // enable the pull-up register
  digitalWrite(BUTTON_PIN, HIGH);
  
  // Identify the routine and conditions for our interrupt. An interrupt
  // can have only one routine attached, and that routine takes no inputs
  // and returns no output.  The specified routine will be called in 
  // the midst of the normal loop processing, so any variables that are
  // changed should probably be marked as volatile, if they are real-time
  // critical.  For our purposes, it's not necessary, but it may be 
  // significant if we have more robust processing of data changed in an 
  // interrupt routine.
  attachInterrupt(INTERRUPT_NUM, toggleLed, CHANGE); 
	
  // start serial port at 9600 bps:
  Serial.begin(9600);

  delay(SENSOR_INIT_DELAY);
}

void loop()
{
  // As noted above, the sensor can only translate data so
  // quickly, so each time through loop, we need to delay 
  // the read of the next sensor value
  delay(SENSOR_DELAY);                  

  // only calculate the distance if the button is pressed
  if (LOW == button_state)
  {
    // read the value from the sensor:
    int sensorValue = analogRead(SENSOR_PIN);
    int dist = getDistance(sensorValue);

    // if the button is being pressed, send the data to the
    // serial interface.
    Serial.print("sensor val = ");
    Serial.print(sensorValue);
    Serial.print("  distance = " );
    Serial.println(dist);
    
    // reset the button state so that we only get one reading
	// per button press
    button_state=HIGH;
  }
}
